#ifndef _CONFIGURATION_H
#define _CONFIGURATION_H

#include <stdint.h>

struct ForceConfiguration {
    double p_gain_motion_translation;
    double p_gain_motion_orientation;
    double d_gain_motion_translation;
    double d_gain_motion_orientation;
    double p_gain_force_translation;
    double p_gain_force_orientation;
    double force_error_damping;
    double compliant_axes_velocity_damping_translation;
    double compliant_axes_velocity_damping_orientation;
    double compliant_axes_speed_limit_gain_translation;
    double compliant_axes_speed_limit_gain_orientation;
    double stiction_gain;
    double joint_softness;
    double minimum_dexterity;
    double minimum_distance_to_point;
    double minimum_tcp_velocity;
    double make_print_to_file;
};

struct JointConfiguration {   // sizeof(struct JointConfiguration) = 300

  int id;

  /* [Joint] section */
  double min_limit;		// offset: 4
  double max_limit;	// offset: 12
  double v_max;	// offset: 20
  double a_max;	// offset: 28
  double torque_max;	// offset: 36
  double gear_ratio;	// offset: 44
  double current_measurement_error_window;	// offset: 52
  double temperatur_model_parameters[4];	// offset: 60
  double teach_stiffness;	// offset: 92
  double v_max_teach_mode;	// offset: 100

  /* Legacy friction model parameters start */
  double static_friction;				// offset: 108
  double dynamic_friction;				// offset: 116
  double viscous_friction;				// offset: 124
  double dynamic_friction_backdrive;							// offset: 132
  double viscous_friction_backdrive;							// offset: 140
  double dynamic_correction_term;								// offset: 148
  double dynamic_correction_term_backdrive;					// offset: 156
  double stiction_positive_qd_calculated_from_selftest;		// offset: 164
  double stiction_negative_qd_calculated_from_selftest;		// offset: 172
  double friction_constant_calculated_from_selftest;		// offset: 180
  double friction_constant_correction_term;					// offset: 188
  double friction_constant_correction_term_backdrive;		// offset: 196
  /* Legacy friction model parameters end */

  /* [Motor] section */
  double rotor_inertia;											// offset: 204
  double torque_constant;										// offset: 212
  double torque_constant_calculated_from_selftest;			// offset: 220
  double winding_resistance;									// offset: 228
  int motor_type; /* 1 == kreipke, 2 == kollmorgen */		// offset: 236

  /* [Units] section */
  int JOINT_ENCODER_TICKS_PER_REVOLUTION;			// offset: 240
  double APOS_U_PER_JOINT_RADIAN;					// offset: 244
  double AVEL_U_PER_JOINT_RADIANS_PER_SECOND;	// offset: 252
  double CUR_U_PER_AMP;								// offset: 260
  double VOL_U_PER_VOLT;							// offset: 268
  double MAX_CURRENT;								// offset: 276
  double MOTOR_TEMP_U_PER_DEGREE;					// offset: 284
  double MOTOR_TEMP_OFFSET;							// offset: 292

};

/* This should be part of the joint struct */
struct Calibration {				// 1908

  int joint_revision[6];  /* 1 == Version 1.3 / Controllerbox 2.0*/								// 24	offset: 0
  int calibration_status;  /* 0 == notInitialized / 1 == notLinearised / 2 == Linearised */		// 4	offset: 24
  uint32_t joint_checksum[6];																	// 24	offset: 28

  double delta_a[6];						// 48	offset: 52
  double delta_d[6];						// 48	offset: 100
  double delta_alpha[6];					// 48	offset: 148
  double delta_theta[6];					// 48	offset: 196
  double encoder_sin_excentricity[6];		// 48	offset: 244
  double encoder_cos_excentricity[6];		// 48	offset: 292

  /* Test results from motor calibration */
  int calibrationDataReadyToBeWrittenToFile;	// 4	offset: 340
  int done;										// 4	offset: 344

  /* Self-test data from here */
  double self_test_measurement_speed[8][6];		// 288	offset: 348
  double self_test_measurement_voltage[8][6];	// 288	offset: 732
  double self_test_measurement_current[8][6];	// 288	offset: 1116
  double self_test_measurement_temp[8][6];		// 288	offset: 1500
  int self_test_completed[6];					// 24	offset: 1884
  /* Self-test data to here */
};

struct Configuration {                   // sizeof(struct Configuration) = 4940

  /* configuration-file path */
  char *path;                            // 4	offset: 0

  /* JointConfiguration */
  struct JointConfiguration joint[6];    // 300*6=1800   offset: 4

  /* [Joint] section */
  double v_joint_default;				// 8	offset: 1804
  double a_joint_default;				// 8	offset: 1812

  /* [Tool] section */
  double tcp_pose[6];					// 48	offset: 1820
  double tcp_payload_mass;				// 8	offset: 1868
  double tcp_payload_cog[3];			// 24	offset: 1876
  double v_tool_default;				// 8	offset: 1900
  double a_tool_default;				// 8	offset: 1908
  double eqradius;						// 8	offset: 1916

  /* [Link] section */
  double mass[6];						// 48	offset:  1924
  double center_of_mass[6][3];			// 144	offset: 1972
  double inertia_matrix[6][3][3];		// 432	offset: 2116
  double gravity[3];					// 24	offset: 2548

  /* [DH] section */
  double a[6];							// 48	offset: 2572
  double d[6];							// 48	offset: 2620
  double alpha[6];						// 48	offset: 2668
  double q_home_offset[6];				// 48	offset: 2716
  int joint_direction[6];				// 24	offset: 2764

  /* [Units] */
  double MASTER_POWERBUS_VOLTAGE_UNITS_PER_VOLT;	// 8	offset: 2788
  double MASTER_POWERBUS_CURRENT_UNITS_PER_AMP;		// 8	offset: 2796
  double MASTER_POWERBUS_CURRENT_UNITS_OFFSET;		// 8	offset: 2804
  double MASTER_TEMP_UNITS_PER_DEGREE;				// 8	offset: 2812
  double MASTER_TEMP_UNITS_OFFSET;					// 8	offset: 2820
  double MASTER_IO_CURRENT_UNITS_PER_AMP;			// 8	offset: 2828
  double MASTER_5V_VOLTAGE_UNITS_PER_VOLT;			// 8	offset: 2836
  double TOOL_ACCELEROMETER_UNITS_PER_M_S_S;		// 8	offset: 2844
  double TOOL_ACCELEROMETER_OFFSET_UNITS;			// 8	offset: 2852

  /* [Config] section */
  int masterboard_version;							// 4	offset: 2860
  int dump_bytecode_on_exception;					// 4	offset: 2864
  int ignore_missing_joints; /* Not set in urcontrol.conf per default */   // 4	   offset: 2868

  /* [Hardware] section */
  int controller_box_type; /* Tells what kind of controller box the controller is running in. (1=CB1 (old white cabinet with emergency stop button), 2=CB2) */  //2872
  int robot_type; /* Tells what kind of robot the controller is connected to. (1=UR5, 2=UR10) */   // 2876
  int robot_sub_type; /* May be used to differentiate between hardware versions */    // 2880

  /* Does not appear in urcontrol.conf */
  int masterboard_detected_version; /* Send by masterboard */               // 2884
  int masterboard_detected_controllerbox_type; /* Send by masterboard */    // 2888

  struct Calibration calibration;					 // 1908 	offset:  2892
  struct ForceConfiguration force;					 // 136	    offset:  4800

  int reloadConf;									// 4
};

extern struct Configuration conf;

#ifdef __cplusplus
extern "C" {
#endif

  int calibration_load();
  int calibration_save();
  int configuration_load();
  void check_for_valid_hw_combo();
  void configuration_print();
  void calibration_print(int);
  const char* getJointVersion(int const jointId);
  void setForceParamsToDefault();

#ifdef __cplusplus
}
#endif

#endif              /* _CONFIGURATION_H */
